import L1_laser.sensor.config as sensor_cfg
class Sensor():
    def __init__(self, model, data):
        self.model = model
        self.data = data
        self.laser_sensor_names = self.gen_L1_sensor_array_name()
    
    # 生成传感器阵列的名称
    def gen_L1_sensor_array_name(self):
        sensor_array_num_size = len(str(abs(sensor_cfg.LASER_SENSOR_NUM)))
        laser_sensor_names = []
        for i in range(0, sensor_cfg.LASER_SENSOR_NUM):
            laser_sensor_names.append(
                sensor_cfg.LASER_SENSOR_NAME + str(i).zfill(sensor_array_num_size)
            )
        return laser_sensor_names
    
    # 获取激光雷达数据，返回一个字典，其中储存当前帧的雷达位置，速度，每个阵列的距离
    def get_laser_sensor_data(self):
        laser_sensor_data = {}
        laser_sensor_data["pos"] = list(self.data.sensor(sensor_cfg.POS_SENSOR_NAME).data)[0]
        laser_sensor_data["range"] = []
        for i in range(0, len(self.laser_sensor_names)):
            laser_sensor_data["range"].append(list(self.data.sensor(self.laser_sensor_names[i]).data)[0])
        return laser_sensor_data
    
    # 获取激光雷达速度
    def get_laser_sensor_vel(self):
        return list(self.data.sensor(sensor_cfg.VEL_SENSOR_NAME).data)[0]